/**
Deng's FOC 闭环位置控制例程 测试库：SimpleFOC 2.1.1 测试硬件：灯哥开源FOC V3.0
在串口窗口中输入：T+位置，就可以使得两个电机闭环转动
比如让两个电机都转动180°，则输入其弧度制：T3.14
在使用自己的电机时，请一定记得修改默认极对数，即 BLDCMotor(14) 中的值，设置为自己的极对数数字
程序默认设置的供电电压为 16.8V,用其他电压供电请记得修改 voltage_power_supply , voltage_limit 变量中的值
默认PID针对的电机是 GB6010 ，使用自己的电机需要修改PID参数，才能实现更好效果
*/
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif

#include <SimpleFOC.h>

uint32_t frequency = 400000;
double init_p_sensor = 0;
double init_p_sensor1 = 0;
int commaPosition = 0; //存储还没有分离出来的字符串
double motor1_angle = 0, motor2_angle = 0;

MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);
MagneticSensorI2C sensor1 = MagneticSensorI2C(AS5600_I2C);
TwoWire I2Cone = TwoWire(0);
TwoWire I2Ctwo = TwoWire(1);

//电机参数
BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(32, 14, 25, 12); // M1电机

BLDCMotor motor1 = BLDCMotor(7);
BLDCDriver3PWM driver1 = BLDCDriver3PWM(33, 26, 27, 13); // M2电机

//命令设置
float target_angle = 0;
// Commander command = Commander(Serial);
// void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }

void task_LED(void *pvParam) // 宣告任务LED闪烁
{
	/* 使用ledc通道1，设置频率为1kHz，分辨率为10位 */
	int freq = 1000;					  // 频率
	int channel = 0;					  // 通道
	int resolution = 8;					  // 分辨率
	ledcSetup(channel, freq, resolution); // 设置通道
	ledcAttachPin(LED_BUILTIN, channel);  // 将通道与对应的引脚连接
	while (1)
	{
		// 逐渐变亮
		for (int dutyCycle = 0; dutyCycle <= 255; dutyCycle = dutyCycle + 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}

		// 逐渐变暗
		for (int dutyCycle = 255; dutyCycle >= 0; dutyCycle = dutyCycle - 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}
	}
}

void setup()
{
	xTaskCreate(task_LED, "task_LED", 1000, NULL, 1, NULL);
	I2Cone.begin(5, 18, frequency);  // CN1 磁编码器
	I2Ctwo.begin(21, 22, frequency); // CN2 磁编码器
	sensor.init(&I2Cone);
	sensor1.init(&I2Ctwo);
	//连接motor对象与传感器对象
	motor.linkSensor(&sensor);
	motor1.linkSensor(&sensor1);

	//供电电压设置 [V]
	driver.voltage_power_supply = 24;
	driver.init();

	driver1.voltage_power_supply = 24;
	driver1.init();
	//连接电机和driver对象
	motor.linkDriver(&driver);
	motor1.linkDriver(&driver1);

	// FOC模型选择
	motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
	motor1.foc_modulation = FOCModulationType::SpaceVectorPWM;
	motor.torque_controller = TorqueControlType::voltage;
	motor1.torque_controller = TorqueControlType::voltage;
	//运动控制模式设置
	motor.controller = MotionControlType::angle;
	motor1.controller = MotionControlType::angle;

	//速度PI环设置
    motor.PID_velocity.P = 0.7;
    motor.PID_velocity.I = 0.5;
    motor.PID_velocity.D = 0.002;
    motor.PID_velocity.output_ramp = 900.0;
    // Low pass filtering time constant 
    //速度低通滤波时间常数
    motor.LPF_velocity.Tf = 0.2;
    motor.PID_velocity.limit = 40.0;

    //速度PI环设置
    motor1.PID_velocity.P = 0.7;
    motor1.PID_velocity.I = 0.5;
    motor1.PID_velocity.D = 0.002;
    motor1.PID_velocity.output_ramp = 900.0;
    // Low pass filtering time constant 
    //速度低通滤波时间常数
    motor1.LPF_velocity.Tf = 0.2;
    motor1.PID_velocity.limit = 40.0;

    //角度P环设置
    motor.P_angle.P = 15.0;
    motor1.P_angle.P = 15.0;
    //最大电机限制电机
    motor.voltage_limit = 24;
    motor1.voltage_limit = 24;

	Serial.begin(115200);
	//需要先连接串口，在初始化过程中的那些量才会打印出来
	motor.useMonitoring(Serial);
	motor1.useMonitoring(Serial);
	//初始化电机
	motor.init();
	motor1.init();
	//初始化 FOC
	motor.initFOC();
	// motor.initFOC(2.3838, Direction::CCW);
	motor1.initFOC();
	// motor1.initFOC();
	Serial.println("Motor ready.");
	init_p_sensor = sensor.getAngle();
	init_p_sensor1 = sensor1.getAngle();
	Serial.println(init_p_sensor);
	Serial.println(init_p_sensor1);

	// command.add('T', doTarget, "target angle");

	Serial.println(F("Motor ready."));
	Serial.println(F("Set the target angle using serial terminal:"));
}
String serialReceiveUserCommand()
{

	// a string to hold incoming data
	static String received_chars;

	String command = "";

	while (Serial.available())
	{
		// get the new byte:
		char inChar = (char)Serial.read();
		// add it to the string buffer:
		received_chars += inChar;

		// end of user input
		if (inChar == '\n')
		{
			Serial.println(command);
			// execute the user command
			command = received_chars;

			//根据逗号分离两组字符串
			//想象中双足轮通讯数据格式:高度,指令   指令格式：英文+值
			commaPosition = command.indexOf(','); //检测字符串中的逗号
			if (commaPosition != -1)			  //如果有逗号存在就向下执行
			{
				motor1_angle = command.substring(0, commaPosition).toDouble();					  //一号电机角度
				motor2_angle = command.substring(commaPosition + 1, command.length()).toDouble(); //打印字符串，从当前位置+1开始
				Serial.println(motor1_angle);
				Serial.println(motor2_angle);
			}
			// reset the command buffer
			received_chars = "";
		}
	}
	return command;
}

void loop()
{
	// Serial.print(sensor.getAngle());
	// Serial.print(" - ");
	// Serial.println(sensor1.getAngle());
	motor.loopFOC();
	motor1.loopFOC();
	motor.move(motor1_angle);
	motor1.move(motor2_angle);
	serialReceiveUserCommand();
	// command.run();
}
